# Copyright (c) # Copyright (c) 2018-2020 CVC.
#
# This work is licensed under the terms of the MIT license.
# For a copy, see <https://opensource.org/licenses/MIT>.


""" This module implements an agent that roams around a track following random
waypoints and avoiding other vehicles. The agent also responds to traffic lights,
traffic signs, and has different possible configurations. """

import random
import numpy as np
import carla

from agents.navigation.agent import Agent
from agents.navigation.AD_RTK_planner import ADRTKPlanner, RoadOption
from agents.navigation.global_route_planner import GlobalRoutePlanner
from agents.navigation.global_route_planner_dao import GlobalRoutePlannerDAO
from agents.navigation.types_behavior import Cautious, Aggressive, Normal

from agents.tools.misc import get_speed, positive, compute_distance
from agents.tools.misc import draw_waypoints, draw_way

from agents.perception.fusion_perception import FusionPerception

class ADRTKAgent(Agent):
    """
    global_planner
    local_planner

    1.碰撞检测
    2.local_planner中，地图中 两点朝向角的变化/两点间距离 = 曲率, 曲率变化率怎么计算
    3.
    """

    def __init__(self, vehicle, ignore_traffic_light=False, behavior='normal'):
        # 1
        """
        Constructor method.

            :param vehicle: actor to apply to local planner logic onto
            :param ignore_traffic_light: boolean to ignore any traffic light
            :param behavior: type of agent to apply
        """
        super(ADRTKAgent, self).__init__(vehicle)

        # for debug
        self._perception = None
        
        self.vehicle = vehicle
        self.ignore_traffic_light = ignore_traffic_light
        self._grp = None
        self.look_ahead_steps = 0

        # Vehicle information
        self.speed = 0
        self.speed_limit = 0
        self.direction = None
        self.incoming_direction = None
        self.incoming_waypoint = None
        self.start_waypoint = None
        self.end_waypoint = None
        self.is_at_traffic_light = 0
        self.light_state = "Green"
        self.light_id_to_ignore = -1
        self.min_speed = 5
        self.behavior = None
        self._sampling_resolution = 0.2
        # 计算最小转弯半径
        vehicle_physics_control = self._vehicle.get_physics_control()
        weels = vehicle_physics_control.wheels
        self._axis_length = compute_distance(weels[0].position, weels[2].position) / 100 # cm -> m
        self._max_steer_angle = weels[0].max_steer_angle / 2 # degree 最大前轮转角,假定内外角一致
        self._min_radius = self._axis_length / np.sin(np.radians(self._max_steer_angle)) # 最小转弯半径

        # Parameters for agent behavior
        self.behavior = Normal()
        self.next_target_speed = 120
        self._local_planner = ADRTKPlanner(self) 
        self._wps = None
        self._display_time = 60

    def set_perception(self, static_obs, moving_obs):
        self._perception = FusionPerception(static_obs, moving_obs)

    def update_information(self, world):
        """
        更新信息用于显示
        This method updates the information regarding the ego
        vehicle based on the surrounding world.

            :param world: carla.world object
        """
        self.speed = get_speed(self.vehicle)
        self.speed_limit = world.player.get_speed_limit()
        self._local_planner.set_speed(self.speed_limit)
        self.direction = self._local_planner.target_road_option
        if self.direction is None:
            self.direction = RoadOption.LANEFOLLOW

        self.look_ahead_steps = int((self.speed_limit) / 10)

        self.incoming_waypoint, self.incoming_direction = self._local_planner.get_incoming_waypoint_and_direction(
            steps=self.look_ahead_steps)
        if self.incoming_direction is None:
            self.incoming_direction = RoadOption.LANEFOLLOW

        self.is_at_traffic_light = world.player.is_at_traffic_light()
        if self.ignore_traffic_light:
            self.light_state = "Green"
        else:
            # This method also includes stop signs and intersections.
            self.light_state = str(self.vehicle.get_traffic_light_state())

    def set_destination(self, start_location, end_location, clean=False):
        """
        传入起始点和结束点的xyz坐标，然后返回全局规划结果
        This method creates a list of waypoints from agent's position to destination location
        based on the route returned by the global router.

            :param start_location: initial position
            :param end_location: final position
            :param clean: boolean to clean the waypoint queue
        """
        if clean:
            self._local_planner.waypoints_queue.clear()
            self._local_planner.smooth_frenetpoints.clear()
        self.start_waypoint = self._map.get_waypoint(start_location)
        self.end_waypoint = self._map.get_waypoint(end_location)
        # 获取全局路径
        route_trace = self._trace_route(self.start_waypoint, self.end_waypoint, display=True)
        # 传递到局部规划器
        self._local_planner.set_global_plan(route_trace)

    def reroute(self, destination_points):
        """
        重规划路径：根据传入目标点，重新规划全局路径
        This method implements re-routing for vehicles approaching its destination.
        It finds a new target and computes another path to reach it.

            :param spawn_points: list of possible destinations for the agent
        """
        print("Target almost reached, setting new destination...")
        new_start = self._local_planner.waypoints_queue[-1][0].transform.location
        destination = destination_points[0].location if destination_points[0].location != new_start else destination_points[1].location
        print("New destination: " + str(destination))
        # 调用
        self.set_destination(new_start, destination)

    def _trace_route(self, start_waypoint, end_waypoint, display=False): 
        """
        实例化全局规划器，调用规划方法并返回全局路径
        This method sets up a global router and returns the
        optimal route from start_waypoint to end_waypoint.

            :param start_waypoint: initial position
            :param end_waypoint: final position
            :return route: list(carla.Waypoint, Road_option)
        """
        # Setting up global router
        if self._grp is None:
            wld = self.vehicle.get_world()
            dao = GlobalRoutePlannerDAO(
                wld.get_map(), sampling_resolution=self._sampling_resolution)
            grp = GlobalRoutePlanner(dao)
            grp.setup(self._min_radius)
            self._grp = grp
        # Obtain route plan
        route = self._grp.trace_route(
            start_waypoint.transform.location,
            end_waypoint.transform.location)

        # 显示规划的路径
        self._wps = [rt[0] for rt in route]
        self.display_global_trace(True)
        # 返回
        return route

    def run_step(self, debug=False):
        """
        Execute one step of navigation.
            :param debug: boolean for debugging
            :return control: carla.VehicleControl
        """
        control = carla.VehicleControl(steer=0, throttle=0) # init
        # 
        target_speed = min(self.behavior.max_speed, self.speed_limit - self.behavior.speed_lim_dist, self.next_target_speed)
        # 主要工作都放到了局部路径规划中
        # 获取感知结果
        (static_obs, moving_obs) = self._perception.run_step()
        # 传入感知结果,运行局部路径规划
        control = self._local_planner.run_step(static_obs, moving_obs, target_speed, debug=debug)
        return control

    def display_global_trace(self, debug=False):
        """
            :param time
            :param debug
        """
        if debug:
            draw_way(self._vehicle.get_world(), self._wps, time=self._display_time)
